#ifndef PUBLISH_H
#define PUBLISH_H

#include <Eigen/Dense>

#include <map>
#include <set>
#include <memory>
#include <string>
#include <fstream>
#include <mutex>

namespace publisher {

class Publisher {
public:
    Publisher(const int& trajectory_id = 0);
    Publisher(const Publisher&) = delete;            // 禁用拷贝构造
    Publisher& operator=(const Publisher&) = delete; // 禁用拷贝赋值
    // 保留移动语义（如需）
    Publisher& operator=(Publisher&&) = default;
    Publisher(Publisher&&) = default;  // 启用移动语义

    void SetOdometryPose(const Eigen::Vector3d& world_pose);  
    void SetCurrentLandmarks(const std::vector<Eigen::Vector3d>& landmark);

    void SetState(const Eigen::Vector3d& world_pose);  
    /**
     * Get all landmarks and local landmarks
     * @param all_landmarks
     * @param local_landmarks
     * @return number of landmarks in map
     */
    std::vector<Eigen::Vector3f> get_landmarks();
    std::vector<Eigen::Vector3d> get_poses();
    std::vector<Eigen::Vector3d> get_gt_poses();
    std::vector<Eigen::Vector3d> get_odometry_poses();
    std::vector<Eigen::Vector3d> get_current_landmarks();
    std::vector<Eigen::Vector3f> get_filter_landmarks();
    
    std::vector<Eigen::Vector3d> get_submap_landmarks();

    std::vector<Eigen::Vector3d> get_submap_world_pose();

    std::vector<Eigen::Vector3d> get_all_poses();

private:
	std::vector<Eigen::Vector3f> landmarks_; 
	std::vector<Eigen::Vector3d> poses_;
    std::vector<Eigen::Vector3d> gt_poses_;	
    std::vector<Eigen::Vector3d> odometry_poses_; 
    std::vector<Eigen::Vector3d> current_landmarks_; 
    std::vector<Eigen::Vector3f> filter_landmarks_; 
    std::vector<Eigen::Vector3d> all_poses_;

    std::mutex state_mutex_;
    std::mutex odometry_mutex_;
    std::mutex current_landmark_mutex_;
    std::mutex predict_pose3d_mutex_;
    std::mutex filter_point_cloud_mutex_;
};

} // namespace vio_2d

#endif
